'''
Created on Mar 5, 2014

@author: Miri
'''

import Project.Main as Main
import Project.Utilities.Utilities as Utilities
import os
import Project.Tracker.TrackerInterface as TrackerInterface
import Project.Control.ControlFunctions as CF
import Project.Control.ControlFunctions as Functions


sourcePoints = []
sourcePoints2 = []
targetPoints = []


def generateWorkspace():  
    if os.path.exists('c:\Sadna\Workspace.txt'):
                os.remove('c:\Sadna\Workspace.txt') 
    f = open('c:\Sadna\Workspace.txt','w')
    f.write("0")#empty file
    f.close()
    
def generateRobot():
    robotArray = TrackerInterface.trace()
    robotNum = len(robotArray)
    if os.path.exists('c:\Sadna\Robot.txt'):
                os.remove('c:\Sadna\Robot.txt')
    f = open('c:\Sadna\Robot.txt','w')
    f.write(str(robotNum) + "\n")#num of robots
    for robot in robotArray:
        f.write (str(len(robot.contourPoints)) + "\n")#num of contour points
        for point in robot.contourPoints:
            point = Utilities.normalization(point.x, point.y, Main.WORKSPACE_SIZE)
            f.write (str(point[0]) + " " + str(point[1]) + "\n")#writes each point
    f.close()

def generateTarget():  
    azimuthArr = Functions.getAzimuth()
    robotArray = TrackerInterface.trace()
    robotNum = len(robotArray)
    if os.path.exists('c:\Sadna\Target.txt'):
                os.remove('c:\Sadna\Target.txt') 
    f = open('c:\Sadna\Target.txt','w')
    f.write(str(robotNum) + "\n")#num of robots
    for i in range (0, len(robotArray)):
            robot = robotArray[i]
            robot.azimuth = azimuthArr[i]
            point = Utilities.normalization(robot.center.getX(), robot.center.getY(), Main.WORKSPACE_SIZE)
            f.write (str(point[0]) + " " + str(point[1]) + " " + str(robot.azimuth) + "\n")#writes each center point
            targetPoints.append(point)
    f.close()
    return robotArray, targetPoints


def generateSource():
    azimuthArr = Functions.getAzimuth()
    robotArray = TrackerInterface.trace()
    robotNum = len(robotArray)
    if os.path.exists('c:\Sadna\Source.txt'):
                os.remove('c:\Sadna\Source.txt') 
    f = open('c:\Sadna\Source.txt','w')
    f.write(str(robotNum) + "\n")#num of robots
    
    for i in range (0, len(robotArray)):
            robot = robotArray[i]
            robot.azimuth = azimuthArr[i]
            point = Utilities.normalization(robot.center.getX(), robot.center.getY(), Main.WORKSPACE_SIZE)
            f.write (str(point[0]) + " " + str(point[1]) + " " +  str(robot.azimuth) + "\n") #writes each center point
            sourcePoints.append(point)
            sourcePoints2.append(robot.center.getX())
    f.close()
    return robotArray, sourcePoints


